#include "Simulation.h"

namespace coolpool
{
	Simulation::Simulation(const std::vector<Entity *> & entities, CollisionTimeFinder * collision_time_finder, CollisionHandler * collision_handler, RestingContactHandler * resting_contact_handler,
			int num_workers)
	{
		collision_handler_ = collision_handler;
		collision_time_finder_ = collision_time_finder;
		entities_container_ = new EntitiesContainer(entities);
		kill_ = false;
		num_workers_ = num_workers;
		resting_contact_handler_ = resting_contact_handler;
		time_step = SIMULATION_INIT_TIME_STEP;

		assert(num_workers > 0);

		init();
	}

	void Simulation::init()
	{
		// Grid setup:
		EntitiesPointers * all = entities_container_->getAll();
		Eigen::Vector2d farthest;
		farthest << 0.0, 0.0;
		Eigen::Vector2d one;
		one << 1.0, 1.0;
		double largest_dynamic = 0.0;

		// Finding the farthest and the largest element.
		const BoundingCylinder * current;
		double current_radius;
		Eigen::Vector2d check_point;
		for (unsigned i = 0; i < all->size(); ++i)
		{
			current = &(*all)[i]->getBoundingCylinder();
			current_radius = current->getRadius();
			check_point = current->getCenter() + (one * current_radius);
			if (distanceFromCenterSqr(check_point) > distanceFromCenterSqr(farthest))
				farthest = check_point;
			if (!(*all)[i]->isStatic() && current->getRadius() > largest_dynamic)
				largest_dynamic = current->getRadius();
		}

		// Scaling the max size, may be changed for different maps, may it helps.
		double cell_size = WORLDCELL_SIZE_SCALE * largest_dynamic;

		// Bounds.
		int width = (farthest[0] / cell_size) + 1;
		int length = (farthest[1] / cell_size) + 1;

		// Building the grid.
		world_grid_ = new WorldGrid(width, length, cell_size, entities_container_);
		world_grid_->buildWorldGrid(true);

		// Preventing segmentation fault before first start.
		std::vector<const Collision *> * firsts_empty = NULL;
		std::vector<const Collision *> * resting_contact_empty = NULL;
		collision_work_collector.updateWorkList(new CollisionPairsQueue(), firsts_empty, resting_contact_empty, 0.0);

		// Start up workers workers:
		DetectionCheckWorker * worker;
		for (int i = 0; i < num_workers_; ++i)
		{
			worker = new DetectionCheckWorker(collision_time_finder_, &collision_work_collector, &done);
			detection_check_workers.push_back(worker);
			worker_threads.push_back(new boost::thread(&DetectionCheckWorker::run, worker));
		}
	}

	Simulation::~Simulation()
	{
		delete collision_handler_;
		delete collision_time_finder_;
		delete entities_container_;
		delete resting_contact_handler_;
		delete world_grid_;
	}

	void Simulation::kill()
	{
		kill_ = true;
	}

	std::vector<IRenderable *> * Simulation::getRenderables()
	{
		EntitiesPointers * all_entities = entities_container_->getAll();
		std::vector<IRenderable *> * renderables = new std::vector<IRenderable *>(all_entities->size());

		for (unsigned i = 0; i < all_entities->size(); ++i)
		{
			(*renderables)[i] = (*all_entities)[i]->getRenderable();
		}

		return renderables;
	}

	void Simulation::enqueueCollision(const Collision * col, std::vector<const Collision *> * firsts, std::vector<const Collision *> * resting_contact)
	{
		// No collision.
		if (!col)
			return;
		// Valid collision.
		else
		{
			switch (col->type)
			{
				case INTERPENETRATION:
				{
					// If the firsts vector is empty it means the current col is becoming the first.
					// If the col collision time is smaller then the firsts, we have a new first collision.
					if (firsts->empty() || *col < *((*firsts)[0]))
					{
						// Remove the current firsts from the heap.
						for (unsigned i = 0; i < firsts->size(); ++i)
						{
							delete (*firsts)[i];
						}

						// Reinits the firsts list.
						firsts->clear();
						firsts->push_back(col);
					}
					// Occasionally happens that even with finer step there will be more collisions at the time.
					else if (*col == *((*firsts)[0]))
					{
						firsts->push_back(col);
					}
					else
						delete col;
					return;
				}
				case RESTING_CONTACT:
				{
					resting_contact->push_back(col);
					return;
				}
				case MOVINGAWAY:
				{
					delete col;
					// Collision was already handled.
					return;
				}
				default:
					// Can not happen.
					assert(false);
					return;
			}
		}
	}

	// Checks for collision between entities.
	void Simulation::findCollisions(std::vector<const Collision *> * firsts, std::vector<const Collision *> * resting_contact, double time_step)
	{
		if (PARRALEL_COLLISION_DETECTION_ENABLED)
		{
			world_grid_->buildWorldGrid(false);
			CollisionPairsQueue * check_list = world_grid_->getCollisionPairs();
			done.clear();
			//			int32_t start_time = getMilliCount();
			{
				// Lock the work collector.
				boost::mutex::scoped_lock sc_lock(*collision_work_collector.getLock());

				// Update the list.
				collision_work_collector.updateWorkList(check_list, firsts, resting_contact, time_step);

				// Notify workers.
				collision_work_collector.getCond()->notify_all();
			}
			//TODO:
			// Some sort of active waiting until the workers are done,
			// probably there is more elegant solution.
			while ((int) done.size() < num_workers_)
			{
				boost::this_thread::sleep(boost::posix_time::milliseconds(1));
			}
			//			std::cout << getMilliSpan(start_time) << std::endl;
		}
		else
		{
			world_grid_->buildWorldGrid(false);
			CollisionPairsQueue * check_list = world_grid_->getCollisionPairs();

			const Collision * col;
			CollisionPair current;
			while (!check_list->empty())
			{
				current = check_list->front();
				check_list->pop();
				col = collision_time_finder_->findCollisionTime(current.first, current.second, time_step, firsts->empty() ? 10000 : (*firsts)[0]->time);
				enqueueCollision(col, firsts, resting_contact);
			}
			delete check_list;
		}
	}

	// Moves the simulation to the next frame.
	// It may take mid steps because of collisions. It handles always just one collision at the time
	// (maybe more if the firsts contains more than one element but usually just one). After first collision ist found
	// the world is moved to the time of collision, which is stored in the leftover variable. The collisions is properly handled
	// and the process is repeated until the mid_step moves to the end of the time step.
	void Simulation::nextStep(double time_step)
	{
		beforeNextStep();
		std::vector<const Collision *> resting_contact;
		std::vector<const Collision *> firsts;
		EntitiesPointers * all_entities = entities_container_->getAll();

		double leftover = time_step;
		while (leftover > 0.0)
		{
			resting_contact.clear();
			firsts.clear();
			findCollisions(&firsts, &resting_contact, leftover);

			// Handle resting_contact.
			if (resting_contact.size() > 0)
				resting_contact_handler_->handleRestingContact(&resting_contact);

			// Free memory
			for (unsigned i = 0; i < resting_contact.size(); ++i)
			{
				delete resting_contact[i];
			}

			if (firsts.empty())
			{
				// No collision happend so just update the entities to the next frame.
				for (unsigned i = 0; i < all_entities->size(); ++i)
				{
					(*all_entities)[i]->onUpdate(leftover);
				}

				leftover = 0.0;
			}
			else
			{
				// Collision was found.
				leftover -= firsts[0]->time;

				// Moving to the given time.
				for (unsigned i = 0; i < all_entities->size(); ++i)
				{
					(*all_entities)[i]->onUpdate(firsts[0]->time);
				}
				//				for (unsigned i = 0; i < (*all_entities).size(); ++i)
				//				{
				//					(*all_entities)[i]->clear();
				//				}

				// Resolving collisions.
				for (unsigned i = 0; i < firsts.size(); ++i)
				{
					collision_handler_->handle(firsts[i]);
					delete firsts[i];
				}
			}

			//			for (unsigned i = 0; i < (*all_entities).size(); ++i)
			//			{
			//				(*all_entities)[i]->clear();
			//			}
		}
	}

	void Simulation::run()
	{
		while (!kill_)
		{
			{
				boost::mutex::scoped_lock sc_lock(lock_);
				nextStep(time_step);
				cond_.notify_one();
				cond_.wait(sc_lock);
			}
		}

		// Destroy the workerse properly.
		for (int i = 0; i < num_workers_; ++i)
		{
			detection_check_workers[i]->kill();
		}

		collision_work_collector.getCond()->notify_all();
		for (int i = 0; i < num_workers_; ++i)
		{
			worker_threads[i]->join();
		}
		for (int i = 0; i < num_workers_; ++i)
		{
			delete worker_threads[i];
			delete detection_check_workers[i];
		}
	}
}
